
#include <cstdlib>
#include <limits>

#include <QList>
#include <QVector3D>

#include "ai/astar.h"

AStar* AStar::s_pkInstance = NULL;

AStar* AStar::instance()
{
    if(!s_pkInstance)
        s_pkInstance = new AStar();

    return s_pkInstance;
}

QLinkedList< const QVector3D* > AStar::run(const Graph& rkGraph,
                                           const QVector3D& rkStart,
                                           const QVector3D& rkEnd)
{
    Q_ASSERT(rkGraph.count() > 2);
    Q_ASSERT(rkGraph.contains(&rkStart) && rkGraph.contains(&rkEnd));

    // Clearing.
    clear();

    // Initialization.
    m_pkGraph = &rkGraph;
    m_pkStart = &rkStart;
    m_pkEnd   = &rkEnd;

    init();

    // Algorithm.
    m_kOpened.insert(m_pkStart);
    m_kGCost.insert(m_pkStart, 0);
    m_kFCost.insert(m_pkStart, estimate(*m_pkStart, *m_pkEnd));

    while(!m_kOpened.isEmpty())
    {
        const QVector3D& rkCurrent = next();

        // Check if some path has been found.
        if(&rkCurrent == m_pkEnd)
            return build();

        // Process current.
        m_kOpened.remove(&rkCurrent);
        m_kClosed.insert(&rkCurrent);

        QSet< const QVector3D* > kNeighbours = neighbours(rkCurrent);
        if(kNeighbours.isEmpty())
            continue;

        foreach(const QVector3D* pkNeighbor, kNeighbours)
        {
            // Check if neighbor is candidate.
            if(m_kClosed.contains(pkNeighbor))
                continue;

            float fScore = f(rkCurrent) + (rkCurrent - *pkNeighbor).length();
            if(!m_kOpened.contains(pkNeighbor) || fScore <= g(*pkNeighbor))
            {
                m_kParents.insert(pkNeighbor, &rkCurrent);
                m_kGCost.insert(pkNeighbor, fScore);
                m_kFCost.insert(pkNeighbor, fScore + estimate(*pkNeighbor, *m_pkEnd));

                // Append the newly opened neighbor.
                if(!m_kOpened.contains(pkNeighbor))
                    m_kOpened.insert(pkNeighbor);
            }
        }
    }

    // No path found.
    return QLinkedList< const QVector3D* >();
}

AStar::AStar()
    : m_pkGraph(NULL),
      m_pkStart(NULL),
      m_pkEnd(NULL)
{
}

AStar::~AStar()
{
    delete s_pkInstance;
}

float AStar::estimate(const QVector3D& rkStart, const QVector3D& rkEnd)
{
    return (rkStart - rkEnd).length();
}

void AStar::init()
{
    m_kClosed.reserve(m_pkGraph->count());
    m_kOpened.reserve(m_pkGraph->count());
    m_kGCost.reserve(m_pkGraph->count());
    m_kFCost.reserve(m_pkGraph->count());
    m_kParents.reserve(m_pkGraph->count());
}

void AStar::clear()
{
    m_pkGraph = NULL;
    m_pkStart = NULL;
    m_pkEnd = NULL;

    m_kClosed.clear();
    m_kOpened.clear();
    m_kGCost.clear();
    m_kFCost.clear();
    m_kParents.clear();
}

const QVector3D& AStar::next() const
{
    const QVector3D* pkCurrentBest = NULL;
    float fCurrentCost = std::numeric_limits< float >::max();

    for(QHash< const QVector3D*, float >::ConstIterator kIt = m_kFCost.begin();
        kIt != m_kFCost.end();
        ++kIt)
    {
        // Check it belongs to opened.
        if(m_kOpened.find(kIt.key()) == m_kOpened.end())
            continue;

        // Check if it's better.
        if(kIt.value() < fCurrentCost)
        {
            pkCurrentBest = kIt.key();
            fCurrentCost = kIt.value();
        }
    }

    Q_ASSERT(pkCurrentBest);

    return *pkCurrentBest;
}

QLinkedList< const QVector3D* > AStar::build() const
{
    Q_ASSERT(m_kParents.contains(m_pkEnd));

    QLinkedList< const QVector3D* > kResult;
    const QVector3D* pkCurrent = m_pkEnd;

    QHash< const QVector3D*, const QVector3D* >::ConstIterator kIt;
    forever
    {
        kResult.prepend(pkCurrent);

        kIt = m_kParents.find(pkCurrent);
        if(kIt == m_kParents.end())
            break;

        pkCurrent = kIt.value();
    }

    return kResult;
}

QSet< const QVector3D* > AStar::neighbours(const QVector3D& rkOrigin) const
{
    QSet< const QVector3D* > kResult;
    QList< const Edge3D* > kEdges = m_pkGraph->values(&rkOrigin);

    Q_ASSERT(kEdges.count());

    foreach(const Edge3D* pkEdge, kEdges)
    {
        if(&(pkEdge->p1()) != &rkOrigin)
        {
            kResult.insert(&(pkEdge->p1()));
            continue;
        }

        if(&(pkEdge->p2()) != &rkOrigin)
            kResult.insert(&(pkEdge->p2()));
    }

    return kResult;
}

float AStar::g(const QVector3D& rkVertex) const
{
    QHash< const QVector3D*, float >::ConstIterator kIt = m_kGCost.find(&rkVertex);
    Q_ASSERT(kIt != m_kGCost.end());

    return kIt.value();
}

float AStar::f(const QVector3D& rkVertex) const
{
    QHash< const QVector3D*, float >::ConstIterator kIt = m_kFCost.find(&rkVertex);
    Q_ASSERT(kIt != m_kFCost.end());

    return kIt.value();
}
